#!/usr/bin/env python
# -*- coding:utf-8 -*-
import numpy as np
import cv2
import glob

w=6
h=4
class StereoCalibration(object):
    def __init__(self,camera):
        # termination criteria
        self.criteria = (cv2.TERM_CRITERIA_EPS +
        cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        self.criteria_cal = (cv2.TERM_CRITERIA_EPS +
        cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-5)

        # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
        self.objp = np.zeros((w*h, 3), np.float32)
        self.objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)

        # Arrays to store object points and image points from all the images.
        self.objpoints = [] # 3d point in real world space
        self.imgpoints_l = [] # 2d points in image plane.
        self.imgpoints_r = [] # 2d points in image plane.
         
        self.cap=cv2.VideoCapture(0)
        self.cap.set(3,1280)
        self.cap.set(4,480)
        if  self.cap.isOpened():
            print("Open Success")
        else:
            print("open Failed")
   
    def start_calibre(self):
	print("Start to calibrate left camera")
        	
        rt, self.M1, self.d1, self.r1, self.t1 = cv2.calibrateCamera(
        	self.objpoints, self.imgpoints_l, self.img_shape, None, None)
         
        print("Start to calibrate right camera")
        rt, self.M2, self.d2, self.r2, self.t2 = cv2.calibrateCamera(
        	self.objpoints, self.imgpoints_r, self.img_shape, None, None)

        print("Start to calibrate stereo camera")
        self.camera_model = self.stereo_calibrate()

    def stop_cap(self):
        self.cap.release()

    def start_cap(self):
        while True:
        #从摄像头读取图片
            sucess,img = self.cap.read()
            
            if sucess:
                img_l = img[0:480,0:640]
                img_r = img[0:480,640:1280]
                gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
                gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
                ret_l, corners_l = cv2.findChessboardCorners(gray_l, (w, h), None)
                ret_r, corners_r = cv2.findChessboardCorners(gray_r, (w, h), None)

                

		if ret_l and ret_r is True:
		   rt = cv2.cornerSubPix(gray_l, corners_l, (11, 11),
				(-1, -1), self.criteria)
		   self.imgpoints_l.append(corners_l)

		    # Draw and display the corners
		   ret_l = cv2.drawChessboardCorners(img_l, (w, h), corners_l, ret_l)

   
		   rt = cv2.cornerSubPix(gray_r, corners_r, (11, 11),
				(-1, -1), self.criteria)
		   self.imgpoints_r.append(corners_r)

		    # Draw and display the corners
		   ret_l = cv2.drawChessboardCorners(img_r, (w, h), corners_r, ret_r)

                   
		   self.objpoints.append(self.objp)
                   print(len(self.imgpoints_r))
                   
 
		cv2.imshow("image_l", img_l)
                cv2.imshow("image_r", img_r)
            if cv2.waitKey(500) == ord('q'):
        	break

        cv2.destroyAllWindows()
        self.img_shape = gray_l.shape[::-1]
        
  


    def read_images(self, cal_path):
        images_right = glob.glob(cal_path + 'RIGHT/*.BMP')
        images_left = glob.glob(cal_path + 'LEFT/*.BMP')
        images_left.sort()
        images_right.sort()

        for i, fname in enumerate(images_right):
            img_l = cv2.imread(images_left[i])
            img_r = cv2.imread(images_right[i])

            gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
            gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)

            # Find the chess board corners
            ret_l, corners_l = cv2.findChessboardCorners(gray_l, (w, h), None)
            ret_r, corners_r = cv2.findChessboardCorners(gray_r, (w, h), None)

            # If found, add object points, image points (after refining them)
            self.objpoints.append(self.objp)

            if ret_l is True:
                rt = cv2.cornerSubPix(gray_l, corners_l, (11, 11),
                (-1, -1), self.criteria)
                self.imgpoints_l.append(corners_l)

            # Draw and display the corners
            #ret_l = cv2.drawChessboardCorners(img_l, (w, h), corners_l, ret_l)
            #cv2.imshow(images_left[i], img_l)
            #cv2.waitKey(500)

            if ret_r is True:
                rt = cv2.cornerSubPix(gray_r, corners_r, (11, 11),
                (-1, -1), self.criteria)
                self.imgpoints_r.append(corners_r)

            # Draw and display the corners
            #ret_r = cv2.drawChessboardCorners(img_r, (w, h), corners_r, ret_r)
            #cv2.imshow(images_right[i], img_r)
            #cv2.waitKey(500)
        self.img_shape = gray_l.shape[::-1]

        rt, self.M1, self.d1, self.r1, self.t1 = cv2.calibrateCamera(
        self.objpoints, self.imgpoints_l, img_shape, None, None)
        rt, self.M2, self.d2, self.r2, self.t2 = cv2.calibrateCamera(
        self.objpoints, self.imgpoints_r, img_shape, None, None)

        

    def stereo_calibrate(self):
        dims = self.img_shape
        flags = 0
        flags |= cv2.CALIB_FIX_INTRINSIC
        # flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
        flags |= cv2.CALIB_USE_INTRINSIC_GUESS
        flags |= cv2.CALIB_FIX_FOCAL_LENGTH
        # flags |= cv2.CALIB_FIX_ASPECT_RATIO
        flags |= cv2.CALIB_ZERO_TANGENT_DIST
        # flags |= cv2.CALIB_RATIONAL_MODEL
        # flags |= cv2.CALIB_SAME_FOCAL_LENGTH
        # flags |= cv2.CALIB_FIX_K3
        # flags |= cv2.CALIB_FIX_K4
        # flags |= cv2.CALIB_FIX_K5

        stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER +
            cv2.TERM_CRITERIA_EPS, 100, 1e-5)
        ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate(
            self.objpoints, self.imgpoints_l,
            self.imgpoints_r, self.M1, self.d1, self.M2,
            self.d2, dims,
            criteria=stereocalib_criteria, flags=flags)

        print('Intrinsic_mtx_1', M1)
        print('dist_1', d1)
        print('Intrinsic_mtx_2', M2)
        print('dist_2', d2)
        print('R', R)
        print('T', T)
        print('E', E)
        print('F', F)



        camera_model = dict([('M1', M1), ('M2', M2), ('dist1', d1),
        ('dist2', d2), ('rvecs1', self.r1),
        ('rvecs2', self.r2), ('R', R), ('T', T),
        ('E', E), ('F', F)])

  
        return camera_model

cal = StereoCalibration(1)
cal.start_cap()
cal.stop_cap()
camera_model = cal.start_calibre()
#关闭摄像头
#cap.release()

